#define motor_loop_delay 10 //编码电机更新位置延时变量，单位毫秒

//麦克纳姆四轮小车四个电机引脚初始化
void car_init(){
  motor1.init();
  motor2.init();
  motor3.init();
  motor4.init();
}

//实时更新并保持小车四个电机的位置
void car_update(){
  motor1.update(); //时刻保持编码电机1的位置
  motor2.update(); //时刻保持编码电机2的位置
  motor3.update(); //时刻保持编码电机3的位置
  motor4.update(); //时刻保持编码电机4的位置
  delay(motor_loop_delay);
}

/*
功能：小车移动函数
函数原型：car_move_distance(float dx, float dy, float dw)

*/
void car_move_distance(float dx, float dy, float dw){
  float distance_lf, distance_rf, distance_lb, distance_rb;
  distance_lf = -dx*0.80 + dy*0.83333 + (dw*0.294*3.1415/180.0)*0.58;
  distance_rf =  dx*0.80 + dy*0.83333 + (dw*0.294*3.1415/180.0)*0.58;
  distance_lb = -dx*0.80 - dy*0.83333 + (dw*0.294*3.1415/180.0)*0.58;
  distance_rb =  dx*0.80 - dy*0.83333 + (dw*0.294*3.1415/180.0)*0.58;
  Serial.print(distance_lf);Serial.print(" | ");
  Serial.print(distance_rf);Serial.print(" | ");
  Serial.print(distance_lb);Serial.print(" | ");
  Serial.println(distance_rb);
  wheel_move_distance(distance_lf, distance_rf, distance_lb, distance_rb);
}

//麦克纳姆小车四轮分别需要转动到的位置底层函数
void wheel_move_distance(float wheel_lf, float wheel_rf, float wheel_lb, float wheel_rb){
  motor1.set_pos(wheel_lf);
  motor2.set_pos(wheel_rf);
  motor3.set_pos(wheel_lb);
  motor4.set_pos(wheel_rb);
  while( (motor1.pos_flag) || motor2.pos_flag || motor3.pos_flag || motor4.pos_flag){
    if(motor1.pos_flag){motor1.pos_loop();}
    if(motor2.pos_flag){motor2.pos_loop();}
    if(motor3.pos_flag){motor3.pos_loop();}
    if(motor4.pos_flag){motor4.pos_loop();}
    delay(motor_loop_delay);
  }
  for(int i=0;i<100;i++){
    car_update();
  }
}

//小车动作测试
void car_action_test(){
  car_move_distance( 0.2, 0.0, 0.0);   //小车前进20厘米
  car_move_distance(-0.2, 0.0, 0.0);   //小车后退20厘米
  car_move_distance(0.0,  0.2, 0.0);   //小车左平移20厘米
  car_move_distance(0.0, -0.2, 0.0);   //小车右平移20厘米
  car_move_distance(0.0,  0.0, 90.0);  //小车左转90度
  car_move_distance(0.0,  0.0, -90.0); //小车右转90度
}
